#include <btl/extra/Simulation/PointCloud.hpp>
#include <btl/Maths/FloatComparison.hpp>
#include <boost/random.hpp>

namespace btl
{
namespace extra
{
namespace simulation
{

void PointCloud::setUniformBox(
      int N, const Eigen::Vector3d& minCoord, const Eigen::Vector3d& maxCoord, boost::mt19937& rng)
{
   assert(N >= 1);
   boost::uniform_real<> x(minCoord(0), maxCoord(0));
   boost::uniform_real<> y(minCoord(1), maxCoord(1));
   boost::uniform_real<> z(minCoord(2), maxCoord(2));

   points.resize(3, N);
   for (int j = 0; j < N; ++j) {
      // note: must not move these into the call (as in Vector3d(x(rng), y(rng)...)
      // because the order in which function parameters are evaluated is undefined,
      // which means we can't totally guarantee the repeatability of the results
      double xx = x(rng);
      double yy = y(rng);
      double zz = z(rng);
      points.col(j) = Eigen::Vector3d(xx, yy, zz);
   }
}

void Motion::setLinearMotion(int N, const Eigen::Isometry3d& start, const Eigen::Isometry3d& end)
{
   assert(N >= 2);

   Eigen::Quaterniond rot0 = Eigen::Quaterniond(start.rotation());
   Eigen::Quaterniond rot1 = Eigen::Quaterniond(end.rotation());
   Eigen::Vector3d pos0 = start.translation();
   Eigen::Vector3d pos1 = end.translation();

   cameraToWorld.clear();
   cameraToWorld.reserve(N);
   for (int i = 0; i < N; ++i) {
      const double alpha = double(i) / double(N-1);

      Eigen::Vector3d t = pos0 + alpha*(pos1 - pos0);

      // interpolate between the start and end poses
      // (linear for the position; spherical-linear for the orientation)
      Eigen::Isometry3d pose
         = Eigen::Translation3d(t)
         * Eigen::Isometry3d(rot0.slerp(alpha, rot1));

      cameraToWorld.push_back(pose);
   }
}

NoisyPointSensor::NoisyPointSensor():
   measurementNoise(0.0),
   nearPlane(1e-6),
   fovX(60 * (M_PI/180.0)),
   resX(640),
   resY(480),
   excludeOutOfFrame(false),
   excludeBehindCamera(false)
{
}

Frame NoisyPointSensor::capture(
      const Eigen::Isometry3d& worldToCamera, const Eigen::Matrix3Xd& points, boost::mt19937& rng) const
{
   // can't excludeOutOfFrame if we don't excludeBehindCamera
   assert(!(excludeOutOfFrame && !excludeBehindCamera));
   assert(measurementNoise >= 0.0);
   assert(nearPlane > 0.0);

   const int N = points.cols();

   Frame f;
   f.worldToCamera = worldToCamera;
   f.indexes.clear();
   f.indexes.reserve(N);

   double extentX, extentY, width, noiseSigma;
   const bool applyNoise = (measurementNoise > 0.0);

   if (excludeOutOfFrame || applyNoise)
   {
      extentX = tan(fovX / 2.0);
      extentY = extentX * (double(resY) / double(resX));
      width = extentX*2.0;
      noiseSigma = measurementNoise * (width / double(resX));
   }
   else
   {
      extentX = 1.0;
      extentY = 1.0;
      width = 1.0;
      noiseSigma = 1.0;
   }

   boost::normal_distribution<> noiseDist(0.0, noiseSigma);
   boost::variate_generator<boost::mt19937,boost::normal_distribution<> > noise(rng, noiseDist);

   // apply camera matrix to all points
   f.measurements.noalias() = worldToCamera * points;

   // normalise measurements,
   // quantise to the image resolution,
   // apply noise and cut off any measurements ouside the image frame
   for (int j = 0; j < N; ++j)
   {
      bool exclude = false;
      Eigen::Vector3d v = f.measurements.col(j);

      if (excludeBehindCamera && (v(2) < nearPlane))
         exclude = true;

      if (!exclude && (excludeOutOfFrame || applyNoise))
      {
         v /= v(2);
         assert(btl::almost_equal(v(2), 1.0));

         if (applyNoise)
         {
            v(0) += noise();
            v(1) += noise();
         }

         if (excludeOutOfFrame)
         {
            if (v(0) < -extentX || v(0) > extentX)
               exclude = true;
            if (v(1) < -extentY || v(1) > extentY)
               exclude = true;
         }
      }

      if (!exclude)
      {
         v.normalize();
         f.indexes.push_back(j);
         f.measurements.col(j) = v;
      }
      else
         f.measurements.col(j).setZero();
   }

   return f;
}

Sequence captureSequence(
      const PointCloud& world, const Motion& motion,
      const NoisyPointSensor& sensor, boost::mt19937& rng)
{
   Sequence seq;
   seq.frames.reserve(motion.cameraToWorld.size());
   for (std::vector<Eigen::Isometry3d>::const_iterator
         poseIt = motion.cameraToWorld.begin();
         poseIt != motion.cameraToWorld.end();
         ++poseIt) {

      seq.frames.push_back(Frame());
      Eigen::Isometry3d worldToCamera = poseIt->inverse();
      seq.frames.back() = sensor.capture(worldToCamera, world.points, rng);
   }
   return seq;
}

} // namespace simulation
} // namespace extra
} // namespace btl
